#include "Alignment.h"
#include "dialogs/AlignmentDialog.h"

#include <cc2sm.h>
#include <filtering.h>
#include <my_point_types.h>
#include <sm2cc.h>
#include <PointProjectionTools.h>
#include <ccPointCloud.h>
#include <ccHObject.h>
#include <ccHObjectCaster.h>

#include <boost/thread/thread.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transformation_from_correspondences.h>
#include <pcl/common/transforms.h>
//#include <pcl/registration/icp.h>
#include <QMessageBox>

typedef pcl::visualization::PointCloudColorHandler<sensor_msgs::PointCloud2> ColorHandler;
typedef ColorHandler::Ptr ColorHandlerPtr;
typedef ColorHandler::ConstPtr ColorHandlerConstPtr;

typedef pcl::visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2> GeometryHandler;
typedef GeometryHandler::Ptr GeometryHandlerPtr;
typedef GeometryHandler::ConstPtr GeometryHandlerConstPtr;
using namespace std;
Alignment::Alignment()
	: BaseFilter(FilterDescription("Alignment", "Alignment", "Alignment", ":/toolbar/PclUtils/icons/register.png", true))
	, m_dialog(0)
	, tgt_cloud(new pcl::PointCloud<pcl::PointXYZ>)
	, src_cloud(new pcl::PointCloud<pcl::PointXYZ>)


{
	icpTransf.setIdentity();
	nPointTransf.setIdentity();

}

Alignment::~Alignment()
{
	if (m_dialog)
		delete m_dialog;
	//QMessageBox::information(NULL, "H11World!", "Alignment destroyed!");


}

int Alignment::checkSelected()
{
	//check if two point clouds are selected
    if (m_selected.size() != 2)
        return -12;
	else
	    return 1;
}

int Alignment::openDialog()
{
	if (!m_dialog)
	{
		m_dialog = new AlignmentDialog(this);
	}
	
	//get  target pointcloud (first one that got selected)
	cc_tgt_cloud = getSelectedEntityAsCCPointCloud(0);

	//get source pc, i.e. the one that gets aligned to target (second one that got selected)
	cc_src_cloud = getSelectedEntityAsCCPointCloud(1);


	int f = m_dialog->exec() ? 1 : 0;
	if (f==0){
		uninit();


	}

	return f;
}

void Alignment::tgtPointPicked(size_t id, float x,float y,float z)
{

	Eigen::Vector3f tgt_pt(x,y,z);

	tgt_points.push_back(tgt_pt);

}

void Alignment::srcPointPicked(size_t id, float x,float y,float z)
{

	Eigen::Vector3f src_pt(x,y,z);

	src_points.push_back(src_pt);


}

void Alignment::nPointTransformation()
{
	pcl::TransformationFromCorrespondences t;
		
	if(tgt_points.size()!=src_points.size() || tgt_points.size() ==0)
		return;

	for(int i=0;i<tgt_points.size();i++)
	{
		t.add(tgt_points.at(i),src_points.at(i),1.0);
	}

	nPointTransf = t.getTransformation().inverse().matrix();

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_out (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*src_cloud,*cloud_xyz_out,nPointTransf);

	src_cloud.swap(cloud_xyz_out);
		//QMessageBox::information(NULL, "H11World!", QString::number(nPointTransf(0,0)) +QString::number(nPointTransf(0,1)) +QString::number(nPointTransf(0,2))+QString::number(nPointTransf(0,3)));
	
	m_dialog->tgt_viewer->removeAllPointClouds();
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> g_color(src_cloud, 0, 255, 0);
	m_dialog->tgt_viewer->addPointCloud(src_cloud,g_color,"src_cloud");

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> r_color(tgt_cloud, 255, 0, 0);
	m_dialog->tgt_viewer->addPointCloud(tgt_cloud,r_color,"tgt_cloud");

}
void Alignment::getParametersFromDialog()
{
	assert(m_dialog);
	if (!m_dialog)
		return;

}

int Alignment::compute()
{


	Eigen::Matrix4f finalTrans = icpTransf*nPointTransf;
	ccGLMatrix matrix;
	
	stringstream ss;
	ss<<nPointTransf;
	QString qs;qs.fromStdString(ss.str());
	//QMessageBox::information(NULL, "H11World!", qs);
	//QMessageBox::information(NULL, "H11World!", QString::number(icpTransf(0,0)) +QString::number(icpTransf(0,1)) +QString::number(icpTransf(0,2))+QString::number(icpTransf(0,3)));


	
	float *transformation = matrix.data();

	transformation[0] = finalTrans(0,0);
	transformation[1] = finalTrans(1,0);
	transformation[2] = finalTrans(2,0);
	transformation[3] = finalTrans(3,0);

	transformation[4] = finalTrans(0,1);
	transformation[5] = finalTrans(1,1);
	transformation[6] = finalTrans(2,1);
	transformation[7] = finalTrans(3,1);

	transformation[8] = finalTrans(0,2);
	transformation[9] = finalTrans(1,2);
	transformation[10] = finalTrans(2,2);
	transformation[11] = finalTrans(3,2);

	transformation[12] = finalTrans(0,3);
	transformation[13] = finalTrans(1,3);
	transformation[14] = finalTrans(2,3);
	transformation[15] = finalTrans(3,3);


	ccHObject* selectedEntity = cc_src_cloud;

	ccHObject* ent = selectedEntity;
	bool lockedVertices;
    ccGenericPointCloud* cloud = ccHObjectCaster::ToGenericPointCloud(ent,&lockedVertices);
	ccHObject* parent=0;


	ent->setGLTransformation(matrix);
	ent->applyGLTransformation_recursive();
	ent->prepareDisplayForRefresh_recursive();


 //   sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2);
 //   pcl::toROSMsg(*src_cloud, *sm_cloud);
	//
	//sm2ccReader reader;
	//ccPointCloud* out_c = new ccPointCloud;
	//reader.setInputCloud(sm_cloud);
	//int good = reader.getAsCC(out_c);
	//if(good!=1){
	//	delete out_c;
	//	return -1;

	//}
	//ccHObject* cloud_container = new ccHObject();

	//cloud_container->addChild(out_c);
	//cloud_container->setName(qPrintable("Aligned"));

	//emit newEntity(cloud_container);
	uninit();

	delete m_dialog;
	m_dialog=0;


	return 1;
}

void Alignment::loadTargetCloud(float x,float y,float z)
{
	sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2);
	cc2smReader converter;
    converter.setInputCloud(cc_tgt_cloud);
	converter.getAsSM(*sm_cloud);
	
	sensor_msgs::PointCloud2::Ptr downsampled_tgt_cloud (new sensor_msgs::PointCloud2);

	pcl::VoxelGrid<sensor_msgs::PointCloud2> voxelgrid;
	voxelgrid.setInputCloud(sm_cloud);
	voxelgrid.setLeafSize(x,y,z);
	voxelgrid.setDownsampleAllData(false);
	voxelgrid.filter(*downsampled_tgt_cloud);

	tgt_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*downsampled_tgt_cloud,*tgt_cloud);

	m_dialog->tgt_viewer->removePointCloud("tgt_cloud");
	m_dialog->tgt_viewer->addPointCloud(tgt_cloud,"tgt_cloud");

	

}



void Alignment::loadSourceCloud(float x,float y,float z)
{
	sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2);
	cc2smReader converter;
    converter.setInputCloud(cc_src_cloud);
	converter.getAsSM(*sm_cloud);

	sensor_msgs::PointCloud2::Ptr downsampled_src_cloud (new sensor_msgs::PointCloud2);

	pcl::VoxelGrid<sensor_msgs::PointCloud2> voxelgrid;
	voxelgrid.setInputCloud(sm_cloud);
	voxelgrid.setLeafSize(x,y,z);
	voxelgrid.setDownsampleAllData(false);
	voxelgrid.filter(*downsampled_src_cloud);
	
	src_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*downsampled_src_cloud,*src_cloud);

	m_dialog->src_viewer->removePointCloud("src_cloud");
	m_dialog->src_viewer->addPointCloud(src_cloud,"src_cloud");


}

void Alignment::icpAlign()
{


	
	//pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputTarget (tgt_cloud);
 	icp.setInputCloud(src_cloud);
	//icp.setTransformationEpsilon(m_dialog->doubleSpinBox_icp_epsilon->value());
	icp.setMaxCorrespondenceDistance(m_dialog->doubleSpinBox_icp_distanceTreshold->value());
	//icp.setEuclideanFitnessEpsilon(m_dialog->doubleSpinBox_icp_euclideanEpsilon->value());
	icp.setMaximumIterations ( m_dialog->spinBox_icpIterations->value());

	Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
	pcl::PointCloud<pcl::PointXYZ>::Ptr reg_result(new pcl::PointCloud<pcl::PointXYZ>);



	//for (int i = 0; i < m_dialog->spinBox_icpIterations->value(); i++)
	//{

	//	// save cloud for visualization purpose
	//	src_cloud = reg_result;

	//	// Estimate
	//	icp.setInputCloud(src_cloud);
	//	//reg.setInputCloud (points_with_normals_src);
	//	//reg.align (*reg_result);
	//	icp.align(*reg_result);

	//		//accumulate transformation between each Iteration

	//	Ti = icp.getFinalTransformation () * Ti;

	//		//if the difference between this transformation and the previous one
	//		//is smaller than the threshold, refine the process by reducing
	//		//the maximal correspondence distance
	//	if (fabs ((icp.getLastIncrementalTransformation () - prev).sum ()) < icp.getTransformationEpsilon ())
	//	  icp.setMaxCorrespondenceDistance (icp.getMaxCorrespondenceDistance () - 0.01);
 //   
	//	prev = icp.getLastIncrementalTransformation ();


	//	m_dialog->lcd_icpIterationCounter->display(i);
	//	m_dialog->update();
	//	// visualize current state
	//	//showCloudsRight(points_with_normals_tgt, points_with_normals_src);
	//}
	icp.align(*reg_result);


	Ti = icp.getFinalTransformation();

	icpTransf=Ti*icpTransf;
	// Transform target back in source frame

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_out (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*src_cloud,*cloud_xyz_out,icpTransf);

	src_cloud.swap(cloud_xyz_out);

	m_dialog->tgt_viewer->removeAllPointClouds();
	QString fitness = QString::number(icp.getFitnessScore());
	QMessageBox::information(NULL, "H11World!", fitness);

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> g_color(src_cloud, 0, 255, 0);
	m_dialog->tgt_viewer->addPointCloud(src_cloud,g_color,"src_cloud");

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> r_color(tgt_cloud, 255, 0, 0);
	m_dialog->tgt_viewer->addPointCloud(tgt_cloud,r_color,"tgt_cloud");

}

void Alignment::uninit()
{
	m_dialog->tgt_viewer->removeAllPointClouds();
	tgt_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	m_dialog->tgt_viewer->addPointCloud(tgt_cloud,"tgt_cloud");
	m_dialog->tgt_viewer->close();

	m_dialog->src_viewer->removeAllPointClouds();
	src_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	m_dialog->src_viewer->addPointCloud(src_cloud,"src_cloud");
	m_dialog->src_viewer->close();


	tgt_points.clear();src_points.clear();

	icpTransf.setIdentity();
	nPointTransf.setIdentity();

}